
function dxx = Bcr4bp_EqmSB1Rot3D(t , xx , aux)
%
% BCR4BP动力学模型，仅状态，SB1Rot，3D
%
% 输入：
% t:            [1x1]            时间(s)
% xx:         [6x1]            状态(km , km/s)
% aux:       [1x1]            参数
%
% 输出：
% dxx:        [6x1]            动力学方程(km , km/s^2)
%
% 时间：2024年06月03日
% 作者：张晨
% 单位：CSU
% 邮箱：chenzhang@csu.ac.cn
%%%%%%%%%%%%%%%%%%%%%%%

% ---------------- 载入参数 ----------------
mu = aux.EMRot.mu;
mus = aux.EMRot.mus;
as = aux.EMRot.as;
wm = aux.SB1Rot.wm;
l_star = aux.dim.SB1Rot_l;

% ---------------- 提取变量 ----------------
x = xx(1);
y = xx(2);
z = xx(3);
vx = xx(4);
vy = xx(5);
vz = xx(6);

% ---------------- 天体位置 ----------------
% 地月系统相对SB1rot相位角
thetam = t * wm;

% B1位置
xb1 = mus / (mus + 1);
yb1 = 0;
zb1 = 0;

% B2位置
xb2 = 0;
yb2 = 0;
zb2 = 0;

% 地球位置
xe = xb1 - mu / as * cos(thetam);
ye = yb1 - mu / as * sin(thetam);
ze = zb1;
% vxe = wm * mu / as * sin(thetam);
% vye = -wm * mu / as * cos(thetam);
% vze = 0;

% 月球位置
xm = xb1 + (1 - mu) / as * cos(thetam);
ym = yb1 + (1 - mu) / as * sin(thetam);
zm = zb1;
% vxm = -wm * (1 - mu) / as * sin(thetam);
% vym = wm * (1 - mu) / as * cos(thetam);
% vzm = 0;

% 太阳位置
xs = - 1 / (mus + 1);
ys = 0;
zs = 0;

% % --------------- E/M 碰撞检测 + 最远地心距检测 ---------------
% earthRMin = 6378;
% moonRMin = 1738;
% earthRMax = 250e4;
% 
% cond1 = sqrt((x - xe)^2 + (y - ye)^2 + (z - ze)^2) < (earthRMin / l_star);
% cond2 = sqrt((x - xe)^2 + (y - ye)^2 + (z - ze)^2) > (earthRMax / l_star);
% cond3 = sqrt((x - xm)^2 + (y - ym)^2 + (z - zm)^2) < (moonRMin / l_star);
% 
% if any([cond1 , cond2 , cond3])
%     dxx = [];
%     return;
% end

% ----------------- SB1rot动力学方程 ----------------
r_ec_3pow = ((x - xe)^2 + (y - ye)^2 + (z - ze)^2)^(1.5);
r_mc_3pow = ((x - xm)^2 + (y - ym)^2 + (z - zm)^2)^(1.5);
r_sc_3pow = ((x - xs)^2 + (y - ys)^2 + (z - zs)^2)^(1.5);

dxx = [vx;
    vy;
    vz;
    x + 2 * vy - (1 - mu) / (mus + 1) * (x - xe) / r_ec_3pow - mu / (mus + 1) * (x - xm) / r_mc_3pow - mus / (mus + 1) * (x - xs) / r_sc_3pow;
    y - 2 * vx - (1 - mu) / (mus + 1) * (y - ye) / r_ec_3pow - mu / (mus + 1) * (y - ym) / r_mc_3pow - mus / (mus + 1) * (y - ys) / r_sc_3pow;
    - (1 - mu) / (mus + 1)* (z - ze) / r_ec_3pow - mu / (mus + 1) * (z - zm) / r_mc_3pow - mus / (mus + 1) * (z - zs) / r_sc_3pow];

end
